# 导入必要的库和模块
import rospy
import time
from action_t import Action_t

# 初始化 Action_t 类，使用仿真模式 "sim"
# state_machine_current_state 在 Action_t 内部初始化为 0
drone = Action_t("sim")

print("开始执行 e1.py: 解锁 -> 延时5秒 -> 上锁 (使用内部状态机和单一标志位)")

# 用于记录上一个已打印入口消息的状态
last_printed_state = -1

# 主循环
while not rospy.is_shutdown():
    # 状态机处理 - 使用 drone.state_machine_current_state
    current_state = drone.state_machine_current_state

    if current_state == 0:  # 解锁状态
        # 仅在首次进入此状态时打印
        if last_printed_state != current_state:
            print("状态 0: 解锁无人机")
            last_printed_state = current_state

        # 执行解锁操作 (无需检查线程状态)
        drone.unlock(use_thread=True)
        
        # 检查控制是否完成
        if drone.control_complete():
            print("无人机已解锁")
            drone.state_machine_current_state = 1  # 跳转到延时状态

    elif current_state == 1: # 延时状态
        # 仅在首次进入此状态时打印
        if last_printed_state != current_state:
            print("状态 1: 开始延时 5 秒...")
            last_printed_state = current_state

        # 执行延时操作 (无需检查线程状态)
        drone.time_sleep(5, use_thread=True) # 启动延时线程
        
        # 检查控制是否完成
        if drone.control_complete():
             print("延时结束")
             drone.state_machine_current_state = 2 # 跳转到上锁状态

    elif current_state == 2:  # 上锁状态
        # 仅在首次进入此状态时打印
        if last_printed_state != current_state:
            print("状态 2: 上锁无人机")
            last_printed_state = current_state

        # 执行上锁操作 (无需检查线程状态)
        drone.lock(use_thread=True)
        
        # 检查控制是否完成
        if drone.control_complete():
            print("无人机已上锁")
            drone.state_machine_current_state = 3  # 跳转到完成状态

    elif current_state == 3:  # 完成状态
        # 仅在首次进入此状态时打印
        if last_printed_state != current_state:
            print("状态 3: 所有动作完成")
            last_printed_state = current_state

    # 短暂休眠，避免CPU占用过高
    time.sleep(0.1)

print("e1.py 执行完毕") 